#include <time.h>
#include <unistd.h>

#include "ekf.h"
#include "display.h"
#include "motion.h"
#include "MarkerLocator.h"

static double r_x=0, r_y=0, r_theta=0;
static EkfSlam *slam;
static MarkerLocator *ml;
static Mat m_frame;
#ifdef GUI
static MapDisplay *disp;
static Mat g_outFrame;
Mat outFrame;
#endif

int msElap(clock_t i)
{
	return (int)(1000*(double)(clock()-i)/CLOCKS_PER_SEC);
}

// iterate the EKF
double slamStep(Mat u, VideoCapture &cap, VideoWriter &record)
{
	double r_dp = u.at<double>(0);
	double r_dth = u.at<double>(1);

	Mat frame;
	cap >> frame;

	ml->UpdateMarkers(frame);
	Mat observations = slam->step(u);

	// find the closest visible marker
	double mindist = 99999;
	for(int i=0; i < observations.rows; i+=2) {
		if(observations.at<double>(i) < mindist)
			mindist = observations.at<double>(i);
	}
	#ifdef GUI
	Mat rmap, mcov;
	if(slam->getMap(rmap, mcov)) {
		for(int i=0; i < rmap.rows; i+=2) {
			double g_x = rmap.at<double>(i);
			double g_y = rmap.at<double>(i+1);

			disp->setGuess(g_x,g_y,i/2);
		}
	}

	Mat robot = slam->getRobot();
	disp->guessRobot(robot.at<double>(0), robot.at<double>(1), robot.at<double>(2));
	const IplImage* dispImage = disp->renderMap();

	// Make sure our mat can hold all the images
	int height = (frame.rows > dispImage->height) ? frame.rows : dispImage->height;
	if(!outFrame.data || outFrame.cols < dispImage->width + frame.cols + 100 || outFrame.rows < height) {
		outFrame = Mat::zeros(height, dispImage->width + frame.cols + 100, frame.type());
		Rect binRoi(100 + dispImage->width, 0, frame.cols, frame.rows);
		Rect markerRoi(0, 0, 100, height);
		ml->SetDrawMats(outFrame(binRoi), outFrame(markerRoi));
	}
	Rect iplRoi(100, 0, dispImage->width, dispImage->height);
	cvarrToMat(dispImage).copyTo(outFrame(iplRoi));
	imshow("EKFSlam", outFrame);
	g_outFrame = outFrame;
	record << outFrame;
	waitKey(10);
	#endif

	return mindist;
}

// This is run by the EKF to collect measurements.
// It should return a blank mat if the the ith marker is not visible.
static Mat scan(int i)
{
	Mat r;
	return ml->GetLocation(i);
}

void startSlam(int v)
{
	FileStorage fs("config.yml", FileStorage::READ);
	Mat cam, dist, rpos, q, s;

	fs["cameraMatrix"] >> cam;
	fs["distMatrix"] >> dist;
	fs["sensorPos"] >> rpos;
	fs["q"] >> q;
	fs["s"] >> s;

	fs.release();

	ml = new MarkerLocator(cam, dist, rpos, 100.);

//	Mat q = (Mat_<double>(2,1) << .01, .1*M_PI/180);
//	Mat s = (Mat_<double>(2,1) << 10, .5*M_PI/180);

	slam = new EkfSlam(&scan, q, s);
	#ifdef GUI
	namedWindow("EKFSlam");
	disp = new MapDisplay("Map");
	waitKey();
	#endif

	Mat u = (Mat_<double>(2,1) << 0, 0);

	int fd = openPort();
	oiStart();
	setSafeMode();
	readDist();
	readAngle();

	VideoCapture cap(0);
	if(!cap.isOpened()) {
		printf("Failed capture\n");
		return;
	}

	VideoWriter record("/tmp/demo.avi", CV_FOURCC('D','I','V','X'), 30, Size(1340,600), true);
	if(!record.isOpened()) {
		printf("Failed writer\n");
		return;
	}

	clock_t t;
	time_t stime;
	while(1) {
		t = clock();
		setDrive(v,v);
		double mindist = 99999;

		while(msElap(t) < 100 && !hitWall()) {
			u.at<double>(0) = (double)readDist();
			u.at<double>(1) = M_PI*(double)readAngle()/180;
			mindist = slamStep(u, cap, record);
		}

		// backup before turn when running into something
		if(hitWall() || mindist < 700) {
			if(hitWall()) {
				// backup
				setDrive(-15,-15);
				t = clock();
				while(msElap(t) < 500) {
					u.at<double>(0) = (double)readDist();
					u.at<double>(1) = M_PI*(double)readAngle()/180;
					slamStep(u, cap, record);
				}
			}

			// turn
			if(time(&stime)%60 < 30) {
				setDrive(-5, 5);
			}
			else
				setDrive(5,-5);

			t = clock();
			while(msElap(t) < 250) {
				u.at<double>(0) = 0;
				u.at<double>(1) = M_PI*(double)readAngle()/180;
				slamStep(u, cap, record);
			}
		}
		if(waitKey(10) == 27)
			break;
	}

	setDrive(0,0);
	imwrite("lastFrame.png", g_outFrame);

	close(fd);

	delete ml;
	delete slam;
	#ifdef GUI
	delete disp;
	#endif
}
